
#include <stdlib.h>

#include "x32.h"
#include "ucos.h"

#define X32_display	peripherals[PERIPHERAL_DISPLAY]
#define X32_leds	peripherals[PERIPHERAL_LEDS]
#define X32_buttons	peripherals[PERIPHERAL_BUTTONS]
#define X32_clock	peripherals[PERIPHERAL_MS_CLOCK]
#define X32_pwm_period peripherals[PERIPHERAL_DPC1_PERIOD]
#define X32_pwm_width peripherals[PERIPHERAL_DPC1_WIDTH]
#define X32_decode  peripherals[PERIPHERAL_ENGINE_DECODED]

// The stack size for a task
#define STK_SIZE 1024

// The priority for different tasks
#define TASK_PRIO_PER  	5
#define TASK_PRIO_ERR   10
#define TASK_PRIO_RES   6   //not sure what the priorities should be
#define TASK_PRIO_INC   6
#define TASK_PRIO_DEC   6

UBYTE err;
int	throttle;
int dec_count;

//int state_a; // State for software decoder
//int state_b; // State for software decoder
//int new_a;
//int new_b;

int soft_button_interrupt = 0xFFFF;

unsigned int stk_task1[STK_SIZE];    // stack for periodical task
unsigned int stk_errtask[STK_SIZE];  // stack for error task
unsigned int stk_restask[STK_SIZE];  // stack for reset button task
unsigned int stk_inctask[STK_SIZE];  // stack for increment button task
unsigned int stk_dectask[STK_SIZE];  // stack for decrement button task

OS_EVENT *sem_error;
OS_EVENT *sem_reset;
OS_EVENT *sem_increment;
OS_EVENT *sem_decrement;

/**
 * @brief isr_buttons: Handle buttons pressed.
 */
void isr_buttons(void)
{
    int buttons_pressed;
    OSIntEnter();
    //printf("Buttons pressed\n");
    //OSTimeDly(1);

    buttons_pressed = X32_buttons;
    // Check if first button is pressed.
    if((buttons_pressed & (1 << 0)) & (soft_button_interrupt & (1 << 0))){
        soft_button_interrupt &= ~(1<<0);
        OSSemPost(sem_increment);
    }
    if((buttons_pressed & (1 << 1)) & (soft_button_interrupt & (1 << 1))){
        soft_button_interrupt &= ~(1 << 1);
        OSSemPost(sem_decrement);
    }
    if((buttons_pressed & (1 << 3)) & (soft_button_interrupt & (1 << 3))){
        soft_button_interrupt &= ~(1<<3);
        OSSemPost(sem_reset);
    }
/*
    switch (X32_buttons) {
    case 0x08: 		// Reset button
        soft_button_interrupt &= ~(1 << 3);
        break;
    case 0x01:		// Increment button
        soft_button_interrupt &= ~(1 << 0);
        break;
    case 0x02: 		// Decrement button
        soft_button_interrupt &= ~(1 << 0);

        OSSemPost(sem_decrement);
        break;

    }
*/
    OSIntExit();
}

void reset_button_task(void *data){
    while(1){
        OSSemPend(sem_reset, WAIT_FOREVER, &err);
        OSTimeDly(1);
        if(X32_buttons & (1<<3)){
            X32_display = 0x00;
            X32_pwm_width = 0;
            exit(0);
        }
        soft_button_interrupt |= (1<<3);
    }
}

void inc_button_task(void *data){
    while(1){
        OSSemPend(sem_increment, WAIT_FOREVER, &err);
        OSTimeDly(1);
        if(X32_buttons & (1<<0))
            throttle++;
        soft_button_interrupt |= (1<<0);
   //     while(OSSemAccept(sem_increment)>0);
    }
}

void dec_button_task(void *data){
    while(1){
        OSSemPend(sem_decrement, WAIT_FOREVER, &err);
        OSTimeDly(1);
        if(X32_buttons & (1<<1))
            throttle--;
    //    while(OSSemAccept(sem_decrement)>0);
        soft_button_interrupt |= (1<<1);
    }
}


/**
 * @brief isr_decoder See de documentation in appendix E.
 */
/*void isr_decoder() {
    // Temporary new states.
    OSIntEnter();
    new_a = peripherals[PERIPHERAL_ENGINE_A];
    new_b = peripherals[PERIPHERAL_ENGINE_B];
    // both signals have changed: error
    if(new_a != state_a && new_b != state_b){
        //printf("Go to error\n");
        OSSemPost(semaphore);
    }
    // both signals are high: update up/down counter
    else if(new_a == 1 && new_b == 1) {
        // b signal comes later: positive direction
        if(state_b == 0)
            dec_count++;
        // a signal comes later: negative direction
        else
            dec_count--;
    }
    //printf("State Changed!!!\n");
    //update state
    state_a = new_a;
    state_b = new_b;
    OSIntExit();
}*/

void isr_error(void)
{
    OSIntEnter();
    OSSemPost(sem_error);
    OSIntExit();
}

/**
 * @brief error_task: Handles errors in the communication.
 * @param data dummy
 */
void error_task(void *data){
    // Wait on error
    while(1){
        OSSemPend(sem_error, WAIT_FOREVER, &err);
        // Led on
        X32_leds |= 0x01;
        // Delay 100 ms
        OSTimeDly(5);
        // Led of
        X32_leds ^= 0x01;

        //printf("Error!!!\n");// Remove the pending tasks (probably generated very shortly after the first one.
        while(OSSemAccept(sem_error)>0);
    }
}

/**
 * @brief periodical_task Updates the SSD and the throttle.
 * @param data
 */
void periodical_task(void *data)
{
    while (1) {
        // Set display and PWM width
        // Small guess how we should display speed + throttle.
        int tmp_th, tmp_cntr;
        tmp_th = throttle;
        tmp_th = (tmp_th >> 2);
        tmp_cntr = dec_count;
        tmp_cntr = (tmp_cntr >> 2);
        dec_count = 0;
        X32_display = tmp_th + (tmp_cntr << 8);

        X32_pwm_width = throttle;

        // Printf function
        //printf("Throttle is: %d\n", throttle);
        // Todo: reduce delay. Used only to test on the simulator.
        OSTimeDly(1);
    }
}

void main()
{
    // Initialize PWM and throttle
    throttle = 50;
    X32_pwm_period = 1024;
    // Initial speed is zero.
    X32_pwm_width = throttle;
    dec_count = 0;
    //
    SET_INTERRUPT_VECTOR(INTERRUPT_BUTTONS, &isr_buttons);
    SET_INTERRUPT_PRIORITY(INTERRUPT_BUTTONS, 5);
    ENABLE_INTERRUPT(INTERRUPT_BUTTONS);

    SET_INTERRUPT_VECTOR(INTERRUPT_ENGINE_ERROR, &isr_error);
    SET_INTERRUPT_PRIORITY(INTERRUPT_ENGINE_ERROR, 10);
    ENABLE_INTERRUPT(INTERRUPT_ENGINE_ERROR);

    /*SET_INTERRUPT_VECTOR(INTERRUPT_ENGINE_A, &isr_decoder);
    SET_INTERRUPT_VECTOR(INTERRUPT_ENGINE_B, &isr_decoder);
    SET_INTERRUPT_PRIORITY(INTERRUPT_ENGINE_A, 10);
    SET_INTERRUPT_PRIORITY(INTERRUPT_ENGINE_B, 10);
    ENABLE_INTERRUPT(INTERRUPT_ENGINE_A);
    ENABLE_INTERRUPT(INTERRUPT_ENGINE_B);*/

    OSInit();

    sem_error = OSSemCreate(0);
    sem_reset = OSSemCreate(0);
    sem_increment = OSSemCreate(0);
    sem_decrement = OSSemCreate(0);
    
    OSTaskCreate(error_task,(void*)0, (void *)(stk_errtask), TASK_PRIO_ERR);
    OSTaskCreate(periodical_task,(void *)1,(void *)(stk_task1),TASK_PRIO_PER);
    OSTaskCreate(reset_button_task,(void*)0, (void *)(stk_restask), TASK_PRIO_RES);
    OSTaskCreate(inc_button_task,(void*)0, (void *)(stk_inctask), TASK_PRIO_INC);
    OSTaskCreate(dec_button_task,(void*)0, (void *)(stk_dectask), TASK_PRIO_DEC);

    OSStart();
}



